#ifndef CSERVO_HPP
#define CSERVO_HPP
#include <stdio.h>
#include <wiringPi.h>

class CServo {
private:
  /* data */
  int mIo;
  int mOpenAngle;
  int mCloseAngle;

public:
  CServo(/* args */);
  ~CServo();
  void Open();
  void Close();
  void SetAngle(int angle);
};

CServo::CServo(/* args */) {
  mIo = 1;
  mOpenAngle = 30;
  mCloseAngle = 150;
  wiringPiSetup(); // 使用wiring编码去初始化GPIO序号

  pinMode(mIo, OUTPUT); // 设置GPIO电气属性为推挽输出
}

CServo::~CServo() {}

void CServo::Open() { SetAngle(mOpenAngle); }

void CServo::Close() { SetAngle(mCloseAngle); }

void CServo::SetAngle(int angle) {
  int hold_time = 500 + angle / 180.0 * 2000;
  for (int i = 0; i < 100; i++) {
    digitalWrite(mIo, 1); // high
    delayMicroseconds(hold_time);
    digitalWrite(mIo, 0); // low
    delayMicroseconds(20000 - hold_time);
  }
}

#endif
